Quadrotor Quickstart
This is a walkthrough example on how to use this library. The whole point is to make it easily extensible, and to allow users to easily swap in different controllers and robotic systems.
We will simulate a quadrotor executing a 3D Lissajous trajectory. In the end, we will produce the following animation:

Define the quadrotor
using ComponentArrays, Plots, LinearAlgebra, Parameters
using RoboticSystems
RS = RoboticSystems
gr() # uncomment for fast (but non-interactive) 3d plots
# plotly() # uncomment for interactive 3d plots. Does not support animations.
## Define parameters of the quad
# start with default parameters, and modify some values
quad_p = ComponentArray(
RS.quadrotor_parameters;
mass=1.5,
k_drag_f = 0.02
) |> RS.initialize_quad_params
## Define the quad's initial condition
quad_ic = ComponentArray(
x = zeros(3),
v = zeros(3),
R = 1.0(I(3)) |> collect,
Ω = zeros(3),
ω = 2*RS.hover_ω(quad_p)
);
##Plot the quadrotor
plot()
RS.plot_quad!(quad_ic, quad_p)
RS.plot_triad!(quad_ic)
RS.plot_iso3d!()
Notes:
- the initial state of the quadrotor is expressed using a
ComponentArray. This is an incredible library, and is the keytool to allow us to compose various equations together. - notice the
|>when we constructedquad_p: we passedquad_pthroughinitialize_quad_params. This is necessary to initialize (and invert) some commonly used matrices, to avoid runtime cost RoboticSystemsprovides some convenient plotting tools.RS.plot_quad!plots a quadrotorRS.plot_triadplots the XYZ axes of a rotation matrix as red (x) green (y) and blue (z) lines. By default, we only use ENU frames, andquad_ic.Ris a rotation matrix from the quadrotor-body fixed frame to to inertial frame.RS.plot_iso3d!adjusts x,y,zlims to make the plot look likeaspect_ratio=1
Define the closed-loop function
Next, we need to define the closed-loop dynamics, by using a feedback controller. The controller used in this example is a geometric controller that uses differential flatness to track a path.
## Define a trajectory
# a Lissajous trajectory
trajectory_params = ComponentArray(
A = [1, 1, 0.2],
ω = [5/8,4/8,6/8],
ϕ = [π/2, 0, 0],
off = [0,0,1.]
)
## Define the controller
controller_params = ComponentArray(
kx = 1.0,
kv = 2.0,
kR = 0.35,
kΩ = 0.15,
m = 1.5,
J = quad_p.J, # this is cheating, but is convenient,
invG = quad_p.invG, # this is also cheating...
g = 9.81,
trajectory_params = trajectory_params
)
function controller(state, controller_params, t)
@unpack trajectory_params = controller_params
## define the desired position, velocity, acceleration, jerk, snap at the current time
xd = RS.lissajous(t, trajectory_params)
vd = RS.lissajous(t, trajectory_params, 1)
ad = RS.lissajous(t, trajectory_params, 2)
jd = RS.lissajous(t, trajectory_params, 3)
sd = RS.lissajous(t, trajectory_params, 4)
## define the desired yaw, yaw rate, and yaw acceleration at current time
ψ = 3.0*t/8
ψd = 3.0/8
ψdd = 0.0
## convert into target quadrotor state, using differential flatness
target = RS.flat_state_to_quad_state(xd, vd, ad, jd, sd, ψ, ψd, ψdd)
## determine required thrust and moments, using geometric controller
fM = RS.geometric_controller(state, target..., controller_params)
# returns a vector [f, M[1], M[2], M[3]]
## determine rotation speed of each motor
control = RS.fM_to_ω(fM, controller_params)
# returns the desired ω of each motor
end
## define the closed loop dynamics
function cl_geometric_quad!(D, state, params, t)
@unpack quad_params, controller_params = params
u = controller(state, controller_params, t)
# define the external wind speed (constant, for simplicity)
w_ext = [1.0, 0, 0.]
# determine the rate of change of state based on control input and other forces/torques
RS.quadrotor!(D, state, quad_params, u, t; wind_ext=w_ext)
endcl_geometric_quad! (generic function with 1 method)You can imagine more complicated wind speeds can be modelled. Additional external forces or torques can be specified in RS.quadrotor!.
Perform the Simulation
We can use the excellent library DifferentialEquations.jl to achieve high performance and extensible ODE solving. RoboticSystems.jl doesnt require you to use DiffEq.jl, but it is written in a way that makes it easy to use DiffEq.jl.
using DifferentialEquations
tspan = (0, 100.0)
# combine all the parameters
params = ComponentArray(
quad_params = quad_p,
controller_params = controller_params
)
# define the ODE problem
prob = ODEProblem(cl_geometric_quad!, quad_ic, tspan, params)
## Solve the problem
sol = solve(prob, Tsit5())retcode: Success
Interpolation: specialized 4th order "free" interpolation
t: 1407-element Vector{Float64}:
0.0
8.92818794202627e-5
0.0009821006736228895
0.003036647653795071
0.005987461553222742
0.009981227065692003
0.015346536913553137
0.022374801108247425
0.031527651450210234
0.04309666110570046
⋮
99.44648738635581
99.51830055207641
99.59025435359021
99.6623959991283
99.7346720414413
99.80701021714177
99.87934537670041
99.95162778939407
100.0
u: 1407-element Vector{ComponentArrays.ComponentVector{Float64, Vector{Float64}, Tuple{ComponentArrays.Axis{(x = 1:3, v = 4:6, R = ViewAxis(7:15, ShapedAxis((3, 3), NamedTuple())), Ω = 16:18, ω = 19:22)}}}}:
ComponentVector{Float64}(x = [0.0, 0.0, 0.0], v = [0.0, 0.0, 0.0], R = [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0], Ω = [0.0, 0.0, 0.0], ω = [2775.641994507828, 2775.641994507828, 2775.641994507828, 2775.641994507828])
ComponentVector{Float64}(x = [5.314168202498758e-11, 6.618016817477421e-19, 1.1707462655765928e-7], v = [1.1904250305974494e-6, 3.703237619185394e-14, 0.002620097552199617], R = [0.9999999999999999 -1.345174389946138e-8 2.840740619749971e-11; 1.3451743899460178e-8 0.9999999999999999 4.2408733280914876e-11; -2.8407406767989885e-11 -4.240873289877319e-11 1.0], Ω = [-1.424028910936937e-6, 9.538749015553652e-7, 0.0003020917746086717], ω = [2769.6637708017015, 2769.6333254750016, 2769.7274239349667, 2769.881392660908])
ComponentVector{Float64}(x = [6.430588420047734e-9, 1.0221621824712808e-13, 1.3901634180329061e-5], v = [1.3096589508115594e-5, 5.15749057071848e-10, 0.02801855339054633], R = [0.9999999999985416 -1.7073990086031888e-6 3.704027727741963e-8; 1.7073990065342194e-6 0.9999999999985408 5.531083130000745e-8; -3.704037176998225e-8 -5.531076802029433e-8 0.9999999999999978], Ω = [-0.0001676988904998352, 0.00011229324454817001, 0.003563766406289183], ω = [2711.2733123698263, 2710.9445097048124, 2711.9514725335343, 2713.6112920266605])
ComponentVector{Float64}(x = [6.152730387193882e-8, 2.630460103102323e-11, 0.00012738322688478812], v = [4.056191893946913e-5, 4.213411197356991e-8, 0.0813049241058527], R = [0.9999999998389366 -1.7916704261929145e-5 1.0444085249215207e-6; 1.7916702627036678e-5 0.9999999998382624 1.5605604241880948e-6; -1.0444364985026708e-6 -1.5605417023542635e-6 0.9999999999982315], Ω = [-0.0015063844722949576, 0.0010078555253564023, 0.012524111091468358], ω = [2586.0719776062792, 2585.0995722666225, 2588.0207539547255, 2592.9085149313996])
ComponentVector{Float64}(x = [2.399414736922327e-7, 6.887480397157932e-10, 0.00046694881020027505], v = [8.050466332231332e-5, 5.456419223776936e-7, 0.14686226287253173], R = [0.9999999969932308 -7.71835515561095e-5 7.483222260377458e-6; 7.718346774671266e-5 0.9999999969585784 1.1192716623832484e-5; -7.48408625578526e-6 -1.1192138925059042e-5 0.9999999999093093], Ω = [-0.005356595983141624, 0.003579202465050727, 0.02808448991260358], ω = [2426.6385479300548, 2424.8488879886604, 2430.101337407199, 2439.0386875009076])
ComponentVector{Float64}(x = [6.724564168636131e-7, 7.511622757822137e-9, 0.0012028747734636092], v = [0.0001365980118498079, 3.4607612545215498e-6, 0.218808579734113], R = [0.9999999714662786 -0.0002367777642008057 3.166480110128531e-5; 0.0002367762617485082 0.9999999708427946 4.7435437320745835e-5; -3.167603244544783e-5 -4.742793808313919e-5 0.9999999983733178], Ω = [-0.01320601689510684, 0.008806624339025883, 0.05226652670552954], ω = [2244.0837414661914, 2241.388472859074, 2249.1141751512314, 2262.4458457882506])
ComponentVector{Float64}(x = [1.6205234883815043e-6, 5.2490119736888214e-8, 0.0025842689137849676], v = [0.0002182601853470752, 1.5164890005470314e-5, 0.2923280081350489], R = [0.999999808664043 -0.0006101190839572103 0.00010209566960987038; 0.0006101034249550979 0.9999998021304877 0.00015332592703933419; -0.0001021891985044737 -0.0001532636074377839 0.9999999830326288], Ω = [-0.026647677738861605, 0.017715694617609228, 0.08708437244740404], ω = [2048.3864060422675, 2044.8175370644092, 2054.8665294295038, 2072.264050028121])
ComponentVector{Float64}(x = [3.574023870732133e-6, 2.7059413835752797e-7, 0.004893227557993308], v = [0.0003410359157332293, 5.154584921048957e-5, 0.36031531944045886], R = [0.9999990079830229 -0.0013821583114021641 0.0002714192135276432; 0.001382047172192771 0.9999989612365603 0.0004092263060416773; -0.0002719845500877032 -0.0004088507840543077 0.9999998794298747], Ω = [-0.04627207712173942, 0.030614801725061026, 0.13219760141112022], ω = [1858.3730910756187, 1854.1762110332422, 1865.9954473225844, 1885.991260170912])
ComponentVector{Float64}(x = [7.552550933284999e-6, 1.1347106504340555e-6, 0.008474902033879373], v = [0.0005354336596651468, 0.00014750761996967763, 0.41771781501307725], R = [0.9999957663899104 -0.0028417469602004045 0.0006258492023571512; 0.0028411519780696917 0.9999955131242422 0.0009495239502824135; -0.0006285446953326485 -0.0009477418021083012 0.999999353356962], Ω = [-0.07124114655082653, 0.04677905702408864, 0.1854006326191675], ω = [1691.0942779759293, 1686.7491382548474, 1699.4808689509675, 1719.3950752547826])
ComponentVector{Float64}(x = [1.5477505742824245e-5, 3.996228415951898e-6, 0.013579265312095327], v = [0.0008479190686686156, 0.0003671773548636568, 0.46060391230336406], R = [0.9999850409304443 -0.005321575414999373 0.0012644304532964242; 0.005319120971147042 0.9999839780526486 0.0019366551112819427; -0.0012747162167099232 -0.0019299005065488656 0.999997325303072], Ω = [-0.09781427934323014, 0.06347871117511751, 0.24094089086996107], ω = [1563.1883757887122, 1559.332602128845, 1571.9906315210626, 1588.4347234478623])
⋮
ComponentVector{Float64}(x = [0.7851517451544646, -0.517385392672054, 0.8546172635563027], v = [0.39163310197634027, 0.4262530973359216, 0.10299872332281823], R = [0.9174726500989798 0.39660299321802506 -0.03082212540752428; -0.39637577236378846 0.9179902253086722 0.013423538491127473; 0.03361822412134675 -9.858544861343937e-5 0.9994347452876822], Ω = [0.016195395211837535, -0.01067879041451169, 0.37460755241732363], ω = [1394.3203328467898, 1394.3042878831836, 1394.579045963247, 1394.605793320351])
ComponentVector{Float64}(x = [0.8124955669288082, -0.4864498430302362, 0.8622293361711004], v = [0.36977279532520463, 0.4352059091314117, 0.10872693311602043], R = [0.9277846709799094 0.3717449924448038 -0.03195725608101555; -0.3715296525454672 0.9283348750066762 0.01265208247359027; 0.034370382367126265 0.00013466033187073157 0.999409157196586], Ω = [0.015791407280119258, -0.010258653967812177, 0.37461021525322796], ω = [1394.028048850515, 1394.016485272783, 1394.2874765273407, 1394.317450782004])
ComponentVector{Float64}(x = [0.8382913263830666, -0.4548295184323272, 0.8702573305910213], v = [0.3471226681013647, 0.4436010104652221, 0.11414620122930227], R = [0.9374427920207689 0.34656860157420016 -0.03303358027869149; -0.34636561457150583 0.9380245731784654 0.011864199141297162; 0.035098067686877614 0.00031968866369217174 0.9993838242912684], Ω = [0.015398124346364844, -0.00980118957477719, 0.374613633679979], ω = [1393.723586981723, 1393.7167365763928, 1393.9840027298558, 1394.0169075602287])
ComponentVector{Float64}(x = [0.8624926355707049, -0.4225413638558157, 0.8786856370671561], v = [0.32371010364342956, 0.4514298640459212, 0.11924167629040458], R = [0.9464418770927354 0.32107385311694037 -0.03404930939177916; -0.3208836237059295 0.9470540474462382 0.011060258529942189; 0.035797694842195964 0.0004579742733317204 0.9993589545904165], Ω = [0.015018150892951181, -0.00930722799245207, 0.3746178772845095], ω = [1393.4087563244316, 1393.4068152236748, 1393.6704342440864, 1393.7059305773992])
ComponentVector{Float64}(x = [0.885021244227626, -0.3896483101912223, 0.8874856518431821], v = [0.2995967742889161, 0.4586733995179064, 0.12399290562134763], R = [0.9547642733367643 0.29529661788918476 -0.035001302381420706; -0.29511947224974827 0.9554054644169835 0.01024176387314975; 0.03646479254516615 0.0005510960179637269 0.9993347887566941], Ω = [0.014654540049389125, -0.008778477231424994, 0.37462298736369576], ω = [1393.0827866455384, 1393.0859111602851, 1393.3459982615384, 1393.38370850299])
ComponentVector{Float64}(x = [0.9058018491647036, -0.3562247748837043, 0.8966239794432744], v = [0.274854650334844, 0.46531340087062634, 0.12838041309049852], R = [0.9623929095194965 0.26928061249779955 -0.03588649871255543; -0.2691168010100979 0.9630615702727037 0.009410491187361085; 0.037094969396757986 0.0006010701499072091 0.9993115664228613], Ω = [0.01431028017766267, -0.008217117786318897, 0.3746289730121695], ω = [1392.7445786082963, 1392.7528915902028, 1393.009585296256, 1393.0491165339365])
ComponentVector{Float64}(x = [0.9247714454651499, -0.3223446398609663, 0.9060657445767731], v = [0.2495570747110416, 0.4713354440230384, 0.13238753965127284], R = [0.9693146321194046 0.2430680684900665 -0.03670230775908731; -0.24291777401512685 0.9700090407983674 0.008568191398224961; 0.03768422285218748 0.0006103700569139124 0.9992895134180778], Ω = [0.01398810961017626, -0.0076255829467529014, 0.3746358139203767], ω = [1392.3932456957973, 1392.4068383929618, 1392.6602947614376, 1392.7012487796726])
ComponentVector{Float64}(x = [0.941880790354409, -0.28807660055025863, 0.9157761977126387], v = [0.2237749158245847, 0.4767292556023974, 0.13600074312825658], R = [0.9755205861565445 0.21669640951359137 -0.037446674727879034; -0.21655975801535698 0.9762388669079732 0.007716476783805456; 0.0382290309038602 0.0005818613470671396 0.9992688364342658], Ω = [0.013690459629632513, -0.007006446339886582, 0.3746434623717763], ω = [1392.0282955943044, 1392.0472281293733, 1392.2976181067504, 1392.3395960537227])
ComponentVector{Float64}(x = [0.9522824709076362, -0.2649361826317294, 0.9224122263670387], v = [0.2062633263509368, 0.4799881787188099, 0.13829092660241488], R = [0.9792746603022855 0.19895827594204113 -0.03790442564551146; -0.1988308093044703 0.9800078097505607 0.007141440095172385; 0.03856748056047362 0.0005431368125075653 0.9992558527628539], Ω = [0.013504393168915533, -0.006574853109672898, 0.3746494592297701], ω = [1391.4458752471082, 1391.4676361479108, 1391.7176801196294, 1391.7577356944648])All of the arguments (e.g. restol, abstol) that you normally pass into ODEProblem or solve can be used. Refer to the DifferentialEquations.jl documentation for more details.
Plot/Animate the solution
plotly()
plot()
RS.plot_quad_traj!(sol, quad_p; tspan=tspan)
plot!(xlims=(-1.5, 1.5), ylims=(-1.5, 1.5), zlims=(0, 1.5))
RS.plot_iso3d!()
RS.plot_project3d!()
Next, we can use Plots.jl's animation tools to construct a simple animation.
gr()
anim = @animate for tm = range(sol.t[1], sol.t[end], 200)
tspan=(sol.t[1], tm)
plot()
RS.plot_quad_traj!(sol, quad_p; tspan=tspan)
plot!(xlims=(-1.5, 1.5), ylims=(-1.5, 1.5), zlims=(0, 1.5))
# rotate camera
plot!(camera = (360 * ((tm - sol.t[1]) / (sol.t[end] - sol.t[1])), 30))
plot!(axis = ([], false), xlabel="", ylabel="", zlabel="")
RS.plot_iso3d!()
RS.plot_project3d!()
end;
gif(anim)To plot any additional plot, you can use sol as a function, for example:
gr()
plot(t-> RS.roll(sol(t).R) * 180 / π, tspan..., label="sim")
plot!(xlabel="time (s)", ylabel="roll (degrees)")